#include "Package.h"
#include <Timer.h>
#include "LED.h"

module SendC {
	provides interface SendControl;
	uses interface AMSend;
	uses interface Packet;
	uses interface AMPacket;
	uses interface LEDControl;
	uses interface CC2420Packet;
}
implementation {

	int counter;
	bool busy;
	message_t pkt;
	Package package;
	uint32_t lastTime;

	event void AMSend.sendDone(message_t * msg, error_t error) {
		if(&pkt == msg) {
			busy = FALSE;
			call LEDControl.Off(LEDState_ModeSend);
		}
	}

	void SendPackage(Package * packPtr, int power, int id) {
		if( ! busy) {
			int i;
			Package * mmptr;
			call CC2420Packet.setPower(&pkt, power);
			mmptr = (Package * )(call Packet.getPayload(&pkt, TOSH_DATA_LENGTH));
			mmptr->id = packPtr->id;
			mmptr->arq = packPtr->arq;
			mmptr->ack = packPtr->ack;
			mmptr->moteID = packPtr->moteID;
			for(i = 0; i < DATA_SIZE; i++) 
				mmptr->data[i] = packPtr->data[i];

			if(call AMSend.send(id, &pkt, TOSH_DATA_LENGTH) == SUCCESS) {
				busy = TRUE;
				call LEDControl.On(LEDState_ModeSend);
			}
		}
	}

	async command void SendControl.sendNewPackage(Package * ptr, int power,
			int id) {
		SendPackage(ptr, power, id);
	}

}